#include "PID.h"
#include <math.h>
double calculate_min_angle_error(double current, double target)
{
	// 1. 计算原始差值
	double raw_diff = target - current;

	// 2. 归一化到[-180, 180]范围
	double normalized_diff = fmod(raw_diff + 180.0, 360.0) - 180.0;

	// 3. 边界修正
	if (normalized_diff <= -180.0)
	{
		normalized_diff += 360.0;
	}
	else if (normalized_diff > 180.0)
	{
		normalized_diff -= 360.0;
	}

	return normalized_diff;
}
void PID_Update(PID_t *p)
{
	p->Error1 = p->Error0;
	p->Error0 = p->Target - p->Actual;

	if (p->Ki != 0)
	{
		p->ErrorInt += p->Error0;
		if (p->ErrorInt > 900)
			p->ErrorInt = 900;
		else if (p->ErrorInt < -900)
			p->ErrorInt = -900;
	}
	else
	{
		p->ErrorInt = 0;
	}

	p->Out = p->Kp * p->Error0 + p->Ki * p->ErrorInt + p->Kd * (p->Error0 - p->Error1);

	if (p->Out > p->OutMax)
	{
		p->Out = p->OutMax;
	}
	if (p->Out < p->OutMin)
	{
		p->Out = p->OutMin;
	}
}
void PID_Update_Angl(PID_t *p)
{
	p->Error1 = p->Error0;
	p->Error0 = calculate_min_angle_error(p->Actual, p->Target);
	if (p->Ki != 0)
	{
		p->ErrorInt += p->Error0;
		if (p->ErrorInt > 900)
			p->ErrorInt = 900;
		else if (p->ErrorInt < -900)
			p->ErrorInt = -900;
	}
	else
	{
		p->ErrorInt = 0;
	}

	p->Out = p->Kp * p->Error0 + p->Ki * p->ErrorInt + p->Kd * (p->Error0 - p->Error1);

	if (p->Out > p->OutMax)
	{
		p->Out = p->OutMax;
	}
	if (p->Out < p->OutMin)
	{
		p->Out = p->OutMin;
	}
}
